#ifndef _KALMAN_FILETER_H
#define _KALMAN_FILETER_H
#include "matrix.h"

// 卡尔曼滤波器结构体
typedef struct
{
    Matrix *A;     // 状态转移矩阵
    Matrix *H;     // 观测矩阵
    Matrix *Q;     // 过程噪声协方差
    Matrix *R;     // 观测噪声协方差
    Matrix *P;     // 状态协方差
    Matrix *X;     // 状态向量
    Matrix *X_pre; // 预测状态
    Matrix *P_pre; // 预测协方差
} KalmanFilter;

// 卡尔曼滤波器函数声明
KalmanFilter *create_kalman_filter(Matrix *A, Matrix *H);
void free_kalman_filter(KalmanFilter *kf);
void kalman_predict(KalmanFilter *kf);
void kalman_update(KalmanFilter *kf, Matrix *Z);
void kalman_filt(KalmanFilter *kf, Matrix *Z);

#endif